#!/bin/bash

# =============================================
# 传感器测试脚本 - 简化版
# 只包含：硬件连接检查、相机测试、激光雷达测试
# =============================================

WORKSPACE_PATH="/home/agilex/Desktop/class1-master/class1_workspace"

# 颜色定义
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
NC='\033[0m'

print_info() { echo -e "${BLUE}[INFO]${NC} $1"; }
print_success() { echo -e "${GREEN}[SUCCESS]${NC} $1"; }
print_warning() { echo -e "${YELLOW}[WARNING]${NC} $1"; }
print_error() { echo -e "${RED}[ERROR]${NC} $1"; }

# 初始化ROS环境
init_ros() {
    source /opt/ros/melodic/setup.bash
    source "$WORKSPACE_PATH/devel/setup.bash"
    print_success "ROS环境初始化完成"
}

# 初始化CAN设备
init_can() {
    print_info "检查CAN设备..."
    
    # 检查CAN设备是否已存在且启动
    if ip link show can0 &>/dev/null; then
        if ip link show can0 | grep -q "UP"; then
            print_info "CAN设备已启动，跳过初始化"
            return 0
        else
            print_info "CAN设备存在但未启动，尝试启动..."
            sudo ip link set can0 up type can bitrate 500000
        fi
    else
        print_info "初始化CAN设备..."
        sudo modprobe gs_usb
        sudo ip link set can0 up type can bitrate 500000
    fi
    
    # 检查结果
    if ip link show can0 | grep -q "UP"; then
        print_success "CAN设备就绪"
        echo "CAN状态:"
        ifconfig can0 | head -2
    else
        print_error "CAN设备初始化失败"
        return 1
    fi
}

# 硬件连接检查
check_hardware() {
    print_info "开始硬件连接检查..."
    
    echo ""
    print_info "检查RealSense相机..."
    if lsusb | grep -q "Intel"; then
        print_success "RealSense相机已连接"
        lsusb | grep "Intel"
    else
        print_error "未检测到RealSense相机"
    fi
    
    echo ""
    print_info "检查CAN设备..."
    if ls /sys/class/net/ | grep -q can; then
        print_success "CAN设备存在"
        ls /sys/class/net/ | grep can
    else
        print_error "未检测到CAN设备"
    fi
    
    echo ""
    print_info "硬件检查完成"
}

# 相机测试函数
camera_test() {
    print_info "开始相机图像测试..."
    
    init_ros
    init_can
    
    print_info "启动RealSense摄像头节点..."
    gnome-terminal -- bash -c "source /opt/ros/melodic/setup.bash; source $WORKSPACE_PATH/devel/setup.bash; roslaunch realsense2_camera rs_camera.launch; exec bash"
    
    sleep 8
    
    print_info "启动RViz进行可视化..."
    gnome-terminal -- bash -c "source /opt/ros/melodic/setup.bash; source $WORKSPACE_PATH/devel/setup.bash; rviz; exec bash"
    
    echo ""
    print_warning "请在RViz中执行以下操作："
    echo "1. 点击左下角 'Add' 按钮"
    echo "2. 选择 'By topic' 标签页"
    echo "3. 找到并添加相机话题："
    echo "   - /camera/color/image_raw (彩色图像)"
    echo "   - /camera/depth/image_rect_raw (深度图像)"
    echo "4. 设置 Fixed Frame 为 'camera_link'"
    echo ""
    
    print_info "检查相机话题..."
    gnome-terminal -- bash -c "source /opt/ros/melodic/setup.bash; source $WORKSPACE_PATH/devel/setup.bash; rostopic list | grep camera; sleep 10; exec bash"
    
    read -p "确认图像显示正常后按回车继续..."
}

# 激光雷达测试函数
lidar_test() {
    print_info "开始激光雷达测试..."
    
    init_ros
    init_can
    
    print_info "启动激光雷达节点..."
    gnome-terminal -- bash -c "source /opt/ros/melodic/setup.bash; source $WORKSPACE_PATH/devel/setup.bash; roslaunch scout_bringup open_rslidar.launch; exec bash"
    
    sleep 5
    
    print_info "检查激光雷达数据..."
    gnome-terminal -- bash -c "source /opt/ros/melodic/setup.bash; source $WORKSPACE_PATH/devel/setup.bash; rostopic echo /scan --noarr | head -30; exec bash"
    
    print_info "激光雷达话题信息："
    gnome-terminal -- bash -c "source /opt/ros/melodic/setup.bash; source $WORKSPACE_PATH/devel/setup.bash; rostopic hz /scan; exec bash"
    
    read -p "确认激光雷达工作正常后按回车继续..."
}

# 清理函数
cleanup() {
    print_info "清理测试环境..."
    pkill -f roslaunch 2>/dev/null
    pkill -f rviz 2>/dev/null
    sleep 2
    print_success "环境清理完成"
}

# 显示主菜单
show_menu() {
    echo ""
    echo "========================================"
    echo "      传感器测试脚本 - 简化版"
    echo "========================================"
    echo "1. 硬件连接检查"
    echo "2. 相机图像测试"
    echo "3. 激光雷达测试"
    echo "4. 清理测试环境"
    echo "5. 退出"
    echo "========================================"
}

# 主函数
main() {
    # 设置陷阱，脚本退出时执行清理
    trap cleanup EXIT
    
    echo "========================================"
    echo "   传感器测试脚本 - 简化版"
    echo "   ROS版本: melodic"
    echo "   工作空间: $WORKSPACE_PATH"
    echo "========================================"
    
    while true; do
        show_menu
        read -p "请选择测试项目 [1-5]: " choice
        
        case $choice in
            1)
                check_hardware
                ;;
            2)
                camera_test
                ;;
            3)
                lidar_test
                ;;
            4)
                cleanup
                ;;
            5)
                print_info "退出测试脚本"
                exit 0
                ;;
            *)
                print_error "无效选择，请重新输入"
                ;;
        esac
        
        echo ""
        read -p "按回车键返回主菜单..."
    done
}

# 运行主函数
main "$@"
